#pragma config(Sensor, in2,    pot1,           sensorPotentiometer)
#pragma config(Sensor, in3,    pot2,           sensorPotentiometer)
#pragma config(Sensor, in4,    pot3,           sensorPotentiometer)
#pragma config(Sensor, dgtl2,  lim1,           sensorTouch)
#pragma config(Sensor, dgtl3,  lim2,           sensorTouch)
#pragma config(Motor,  port2,           motor1,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           motor2,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           servo1,        tmotorServoStandard, openLoop)
#pragma config(Motor,  port5,           servo2,        tmotorServoStandard, openLoop)
#pragma config(Motor,  port6,           motor3,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port7,           motor4,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           servo3,        tmotorServoStandard, openLoop)
#pragma config(Motor,  port9,           servo4,        tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void stepServo(tMotor motorPort, float t1) //move servo at given speed
{
	motor[motorPort] = motor[motorPort] + t1;
  wait1Msec(1);
}

void limMotor(tMotor mot1, tSensors sens1, int t2) //stop motor if limswitch pressed
{
	if(SensorValue[sens1] == 1)
		{
		motor[mot1] = t2;
		}
}

int getPotentiometer(tSensors pot)
{
	int t3 = SensorValue[pot];
	return t3;
}

void clawGoToPosition(tMotor first, tMotor second, tMotor third, tMotor fourth,
											int t4, int t5, int t6, int t7)
{
	motor[first] = t4;
	motor[second] = t5;
	motor[third] = t6;
	motor[fourth] = t7;
}

void moveAtAngle(tMotor motA,
								 tSensors potA,
								 int angle,
								 int speedA,
								 int pointErrorA,
								 int startPositionA,
								 float divideAngle1,
								 float divideAngle2,
								 float divideSpeed1,
								 float divideSpeed2)
{
	int z1 = startPositionA + angle*divideAngle1;
	int z2 = startPositionA + angle*divideAngle2;
	while(SensorValue[potA] > (startPositionA + angle) + pointErrorA ||  SensorValue[potA] < (startPositionA + angle) - pointErrorA)
		{
	  if(SensorValue[potA] < startPositionA + angle)
	  	{
			if(SensorValue[potA] < z1)
			{
				motor[motA] = speedA;
			}
			if(SensorValue[potA] < z1 + z2 - startPositionA &&
			 SensorValue[potA] > z1)
			{
				motor[motA] = speedA*divideSpeed1;
			}
			if(SensorValue[potA] > z1 + z2 - startPositionA)
			{
				motor[motA] = speedA*divideSpeed2;
			}
	  	}
	  if(SensorValue[potA] > startPositionA + angle)
	  	{
			if(SensorValue[potA] < angle + z2)
			{
				motor[motA] = -speedA*divideSpeed2;
			}
			if(SensorValue[potA] < angle + z1 &&
			   SensorValue[potA] > angle + z2)
			{
				motor[motA] = -speedA*divideSpeed1;
			}
			if(SensorValue[potA] > angle + z1)
			{
				motor[motA] = -speedA;
			}
	  	}
		}
	motor[motA] = 0;
}

void moveToPosition(tMotor motP,
										tSensors potP,
										int target,
										int speedP,
										int pointErrorP,
										int trajDiff1,
										int trajDiff2,
										float divideSpeed1,
										float divideSpeed2)
{
	while(SensorValue[potP] > target + pointErrorP || SensorValue[potP] < target - pointErrorP)
	{
		if(SensorValue[potP] < target)
		{
			if(SensorValue[potP] < target - trajDiff1)
				{
					motor[motP] = -speedP;
				}
			if(SensorValue[potP] > target - trajDiff1 && SensorValue[potP] < target - trajDiff2)
				{
					motor[motP] = -speedP*divideSpeed1;
				}
			if(SensorValue[potP] > target - trajDiff2)
				{
					motor[motP] = -speedP*divideSpeed2;
				}
		}

		if(SensorValue[potP] > target)
		{
		if(SensorValue[potP] > target + trajDiff1)
			{
				motor[motP] = speedP;
			}
		if(SensorValue[potP] < target + trajDiff1 && SensorValue[potP] > target + trajDiff2)
			{
				motor[motP] = speedP*divideSpeed1;
			}
		if(SensorValue[potP] < target + trajDiff2)
			{
				motor[motP] = speedP*divideSpeed2;
			}
		}
		}
	motor[motP] = 0;
}

void twoServos (tMotor servoPort1, tMotor servoPort2, int Btn1) //control two servos with group 7 buttons
{
	switch (Btn1)
   {
   	case 4:
   	stepServo(servoPort1, 1);
   	break;

   	case 8:
   	stepServo(servoPort2, -1);
   	break;

   	case 16:
   	stepServo(servoPort1, -1);
   	break;

    case 32:
   	stepServo(servoPort2, 1);
   	break;

   	default:
   	wait1Msec(3);
   }
}

void clawOpenCloseCenter(tMotor leftFinger,
									 			 tMotor rightFinger,
									 			 tMotor servoUpDown,
									 			 tMotor servoLeftRight,
									 			 int speedB,
									 			 int Btn2)
{
	switch(Btn2)
		{
		case 1:
		stepServo(leftFinger, -speedB);
   	break;

   	case 2:
   	stepServo(leftFinger, speedB);
   	break;

   	case 64:
   	stepServo(rightFinger, speedB);
    break;

    case 65:
    stepServo(leftFinger, -speedB);
    stepServo(rightFinger, speedB);
    break;

    case 66:
    stepServo(leftFinger, speedB);
    stepServo(rightFinger, speedB);
    break;

    case 128:
    stepServo(rightFinger, -speedB);
    break;

    case 129:
    stepServo(leftFinger, -speedB);
    stepServo(rightFinger, -speedB);
    break;

    case 130:
    stepServo(leftFinger, speedB);
    stepServo(rightFinger, -speedB);
    break;

    case 3840:
    motor[servoUpDown] = 0;
    motor[servoLeftRight] = 0;
    break;

    default:
   	wait1Msec(3);
		}
}

void extendArm(tMotor igusMotor,
							 int speedI,
							 int Btn3)
{
	switch(Btn3)
		{
		case 4:
		motor[igusMotor] = speedI;
		break;

		case 16:
		motor[igusMotor] = -speedI;
		break;

		default:
		motor[igusMotor] = 0;
		}
}

void autonomousFcns(int Btn4, int startPos)
{
	switch(Btn4)
		{
		case 256: //8U
		moveAtAngle(motor1, pot1, 15, 500, 3, startPos, 0.5, 0.25, 0.75, 0.5);
		break;

		case 1024: //8D
		moveToPosition(motor2, pot2, 2048, 900, 7, 1024, 512, 0.7, 0.35);
		break;
		}
}


 int BtnX;

task main()
{
 int x = SensorValue[pot1];
 int y = SensorValue[pot2];

 while(true)
  {
  	motor[motor2] = vexRT[Ch4]; //base motor controlled by joystick ch4
  	motor[motor1] = vexRT[Ch3]; //elbow motor controlled by joystick ch3

    //dead zones for motors
  	if(abs(vexRT[Ch3]) < 5)
  		{
  		motor[motor1] = 0;
  		}

  	if(abs(vexRT[Ch4]) < 5)
  		{
  		motor[motor2] = 0;
  		}

		limMotor(motor3, lim1, 0);

		BtnX = 0;

   //assign values to buttons to change value of BtnX
   if (vexRT[Btn5U] == 1){
     BtnX = BtnX + 1;}

   if (vexRT[Btn5D] == 1){
     BtnX = BtnX + 2;}

   if (vexRT[Btn7U] == 1){
     BtnX = BtnX + 4;}

   if (vexRT[Btn7L] == 1){
     BtnX = BtnX + 8;}

   if (vexRT[Btn7D] == 1){
     BtnX = BtnX + 16;}

   if (vexRT[Btn7R] == 1){
     BtnX = BtnX + 32;}

   if (vexRT[Btn6U] == 1){
     BtnX = BtnX + 64;}

   if (vexRT[Btn6D] == 1){
     BtnX = BtnX + 128;}

   if (vexRT[Btn8U] == 1){
     BtnX = BtnX + 256;}

   if (vexRT[Btn8L] == 1){
     BtnX = BtnX + 512;}

   if (vexRT[Btn8D] == 1){
     BtnX = BtnX + 1024;}

   if (vexRT[Btn8R] == 1){
     BtnX = BtnX + 2048;}

   if ((vexRT[Btn7D] == 0)&&(vexRT[Btn7U] == 0)&&(vexRT[Btn7L] == 0)&&(vexRT[Btn7R] == 0)
     &&(vexRT[Btn8D] == 0)&&(vexRT[Btn8U] == 0)&&(vexRT[Btn8L] == 0)&&(vexRT[Btn8R] == 0)
     &&(vexRT[Btn5U] == 0)&&(vexRT[Btn5D] == 0)&&(vexRT[Btn6U] == 0)&&(vexRT[Btn6D] == 0)){
     BtnX = 0;}

   //twoServos (servo1, servo2, BtnX);
   clawOpenCloseCenter(servo1, servo2, servo3, servo4, 1, BtnX);
   extendArm(motor1, 63, BtnX);
   autonomousFcns(BtnX, x);

   //joystick ch1 controls servo4
    if((vexRT[Ch1] <= -40) && (vexRT[Ch1] > -80))
		{
			stepServo(servo4, -1);
		}
		else if((vexRT[Ch1] <= -80) && (vexRT[Ch1] > -110))
		{
			stepServo(servo4, -2);
		}
		else if(vexRT[Ch1] <= -110)
		{
			stepServo(servo4, -3);
		}

		if((vexRT[Ch1] >= 40) && (vexRT[Ch1] < 80))
		{
			stepServo(servo4, 1);
		}
		else if((vexRT[Ch1] >= 80) && (vexRT[Ch1] < 110))
		{
			stepServo(servo4, 2);
		}
		else if(vexRT[Ch1] >= 110)
		{
			stepServo(servo4, 3);
		}

		//joystick ch2 controls servo3
    if((vexRT[Ch2] <= -40) && (vexRT[Ch2] > -80))
		{
			stepServo(servo3, -1);
		}
		else if((vexRT[Ch2] <= -80) && (vexRT[Ch2] > -110))
		{
			stepServo(servo3, -2);
		}
		else if(vexRT[Ch2] <= -110)
		{
			stepServo(servo3, -3);
		}

		if((vexRT[Ch2] >= 40) && (vexRT[Ch2] < 80))
		{
			stepServo(servo3, 1);
		}
		else if((vexRT[Ch2] >= 80) && (vexRT[Ch2] < 110))
		{
			stepServo(servo3, 2);
		}
		else if(vexRT[Ch2] >= 110)
		{
			stepServo(servo3, 3);
		}
	}
}
